#include "MainWindow.h"


using namespace std::chrono_literals;
using namespace std::string_literals;

MainWindow::MainWindow(QWidget* parent)
    : QMainWindow(parent)
{
    this->ui.setupUi(this);
    connect(this->ui.listWidget, &QListWidget::itemDoubleClicked, this, &MainWindow::_SlotOnListWidgetItemDoubleClicked);
    connect(this->ui.listWidget, &QListWidget::itemPressed, this, &MainWindow::_SlotOnListWidgetItemPressed);
    this->_Init();
}

void MainWindow::_SlotOnPushButtonClicked()
{
    std::cout << "void MainWindow::_SlotOnPushButtonClicked()" << '\n';
}

MainWindow::~MainWindow()
{

}

void MainWindow::SetCurrentEgoCar()
{
    auto vehicles_ = this->_carlaWorld->GetActors()->Filter("vehicle.*");
    // auto std::map<std::string, // 不用 maping 了
    auto id_list_ = QStringList();
    for (size_t i_ = 0; i_ < vehicles_->size(); i_++) {
        auto vehicle_ = vehicles_->at(i_);
        id_list_.append(QString(vehicle_->GetId()));
    }
    auto is_select_ = false;
    auto actor_id_ = QInputDialog::getItem(
        this,
        "请选择要控制的主车",
        "请选择要控制的主车",
        id_list_,
        0,
        false,
        &is_select_
    );
    if (!is_select_) {
        return;
    }
    this->_currentEgoCar = boost::static_pointer_cast<carla::client::Vehicle>(
        this->_carlaWorld->GetActor(carla::ActorId(actor_id_.toInt()))
    );
}

void MainWindow::GetCurrentEgoCarEnv()
{
    // std::cerr << "GetCurrentEgoVarEnv Except\n";
    if (!this->_currentEgoCar) {
        LOG(ERROR) << "当前的主车为空\n";
        return;
    }
    if (!(this->_currentEgoCar->IsAlive()) && (this->_currentEgoCar->IsActive())) {
        LOG(ERROR) << "当前主车没有激活\n";
        return;
    }
    auto wp_ = this->_carlaMap->GetWaypoint(
        this->_currentEgoCar->GetLocation(),
        false
    );
    auto ss_ = std::stringstream();
    ss_ << "当前车辆最近的 waypoint id: " << wp_->GetId()
        << "" << wp_

}

void MainWindow::_Init()
{
    this->_carlaClient = std::make_shared<carla::client::Client>("127.0.0.1", 2000);
    this->_carlaClient->SetTimeout(10s);
    this->_carlaWorld = std::make_shared<carla::client::World>(this->_carlaClient->GetWorld());
    this->_carlaMap = this->_carlaWorld->GetMap();

    // QListWidgetItem item_("this->_carlaMap->GetAllCrosswalkZones()");
    this->ui.listWidget->addItem("glogTest");
    this->ui.listWidget->addItem("GetAllCrosswalkZones");
    this->ui.listWidget->addItem("GetAllLandmarks");
    this->ui.listWidget->addItem("GetAllLandmarksOfType");
    this->ui.listWidget->addItem("GetGeoReference");
    this->ui.listWidget->addItem("GenerateWaypoints");
    this->ui.listWidget->addItem("GenerateOpenDriveWorld");
    this->ui.listWidget->addItem("GetAvailableMaps");
    this->ui.listWidget->addItem("LoadWorld");
    this->ui.listWidget->addItem("GetAndShowRecommendedSpawnPoints");
    this->ui.listWidget->addItem("SpawnACarInSelectedPoint");
    this->ui.listWidget->addItem("DestoryAllCar");
    this->ui.listWidget->addItem("SpawnACarInTargetTransform");
    this->ui.listWidget->addItem("GetAllActorsInfo");
    this->ui.listWidget->addItem("设置当前要控制的主车");
    this->ui.listWidget->addItem("获取当前主车周围的环境信息");
}

void MainWindow::_DeInit()
{

}

void MainWindow::_SlotOnListWidgetItemDoubleClicked(QListWidgetItem* item)
{
    auto double_clicked_item_text_ = item->text().toStdString();
    if (double_clicked_item_text_ == "GetAllCrosswalkZones") {
        auto res_ = this->_carlaMap->GetAllCrosswalkZones();
        auto ss_ = std::stringstream();
        ss_ << "=====获取所有的交叉区域，长度 " << res_.size() << "\n";
        for (size_t i_ = 0; i_ < res_.size(); i_++) {
            ss_ << "/n----GetAllCrosswalkZones [" << i_ << "]" << "\n";
            ss_ << "x = " << res_.at(i_).x << "\n";
            ss_ << "y = " << res_.at(i_).x << "\n";
            ss_ << "z = " << res_.at(i_).x << "\n";
            ss_ << "Abs().x = " << res_.at(i_).Abs().x << "\n";
            ss_ << "Abs().y = " << res_.at(i_).Abs().y << "\n";
            ss_ << "Abs().z = " << res_.at(i_).Abs().z << "\n";
            ss_ << "Length() = " << res_.at(i_).Length() << "\n";
            ss_ << "SquaredLength() = " << res_.at(i_).SquaredLength() << "\n";
            ss_ << "MakeUnitVector().x = " << res_.at(i_).MakeUnitVector().x << "\n";
            ss_ << "MakeUnitVector().y = " << res_.at(i_).MakeUnitVector().y << "\n";
            ss_ << "MakeUnitVector().z = " << res_.at(i_).MakeUnitVector().z << "\n";
            ss_ << "MakeSafeUnitVector(1.5).x = " << res_.at(i_).MakeSafeUnitVector(1.5).x << "\n";
            ss_ << "MakeSafeUnitVector(1.5).y = " << res_.at(i_).MakeSafeUnitVector(1.5).y << "\n";
            ss_ << "MakeSafeUnitVector(1.5).z = " << res_.at(i_).MakeSafeUnitVector(1.5).z << "\n";
        }
        LOG(INFO) << ss_.str();
    } else if (double_clicked_item_text_ == "GetAllLandmarks") {
        auto res_ = this->_carlaMap->GetAllLandmarks();
        auto ss_ = std::stringstream();
        ss_ << "====获取所有交通标志，长度 " << res_.size() << "\n";
        for (size_t i_ = 0; i_ < res_.size(); i_++) {
            ss_ << "/n----GetAllLandmarks [" << i_ << "]" << "\n";
            ss_ << "GetCountry() = " << res_[i_]->GetCountry() << "\n";
            ss_ << "GetDistance() = " << res_[i_]->GetDistance() << "\n";
            ss_ << "GetHeight() = " << res_[i_]->GetHeight() << "\n";
            ss_ << "GethOffset() = " << res_[i_]->GethOffset() << "\n";
            ss_ << "GetId() = " << res_[i_]->GetId() << "\n";
            ss_ << "GetName() = " << res_[i_]->GetName() << "\n";
            ss_ << "GetOrientation() = " << res_[i_]->GetOrientation() << "\n";
            ss_ << "GetPith() = " << res_[i_]->GetPitch() << "\n";
            ss_ << "GetRoadId() = " << res_[i_]->GetRoadId() << "\n";
            ss_ << "GetRoll()" << res_[i_]->GetRoll() << "\n";
            ss_ << "GetS() = " << res_[i_]->GetS() << "\n";
            ss_ << "GetSubType() = " << res_[i_]->GetSubType() << "\n";
            ss_ << "GetT() = " << res_[i_]->GetT() << "\n";
            ss_ << "GetText() = " << res_[i_]->GetText() << "\n";
            ss_ << "GetTransform().location.x = " << res_[i_]->GetTransform().location.x << "\n";
            // TODO 要添加详细的 location 和 ratation
            ss_ << "GetType() = " << res_[i_]->GetType() << "\n";
            ss_ << "GetUnit() = " << res_[i_]->GetUnit() << "\n";

            auto validities_ = res_[i_]->GetValidities();
            ss_ << "GetValidities() 长度 = " << validities_.size() << "\n";
            for (size_t j_ = 0; j_ < validities_.size(); j_++) {
                ss_ << "GetValidities[" << i_ << "].GetValidities()[" << j_ << "]._from_lane = " << validities_[j_]._from_lane << "\n";
                ss_ << "GetValidities[" << i_ << "].GetValidities()[" << j_ << "]._to_lane = " << validities_[j_]._to_lane << "\n";
            }
            ss_ << "GetValue() = " << res_[i_]->GetValue() << "\n";
            
            // 推测是 LandMark 连接一个 waypoint, 这个 waypoint 连接多个 landmark
            // TODO 这里的 GetWaypoint() 可能拿到的是空指针，会报错
            // 从该交通标志起获取多少距离内的其他交通标志列表
            // auto waypoint_ = res_[i_]->GetWaypoint();
            // auto waypoint_landmarks_ =  waypoint_->GetAllLandmarksInDistance(12);
            // ss_ << "GetWaypoint().GetAllLandmarksInDistance(12) 得到的 landmark 长度 " << waypoint_landmarks_.size() << "\n";

            ss_ << "GetWidth() = " << res_[i_]->GetWidth() << "\n";
            ss_ << "GetZOffset() = " << res_[i_]->GetZOffset() << "\n";
            ss_ << "GetDynamic() = " << res_[i_]->IsDynamic() << "\n";
        }
        LOG(INFO) << ss_.str();
    } else if (double_clicked_item_text_ == "GetAllLandmarksOfType") {
        LOG(INFO) << "GetAllLandmarksOfType 获取指定路标名称的所有路标" << std::endl;
    } else if (double_clicked_item_text_ == "GetGeoReference") {
        auto res_ = this->_carlaMap->GetGeoReference();
        auto ss_ = std::stringstream();
        ss_ << "\n====GetGeoReference 获取地理参考" << "\n";
        ss_ << "altitude 高度 = " << res_.altitude << "\n";
        ss_ << "latitude 维度 = " << res_.latitude << "\n";
        ss_ << "longitude 经度 = " << res_.longitude << "\n";
        LOG(INFO) << ss_.str();
    } else if (double_clicked_item_text_ == "GenerateWaypoints") {
        auto res_ = this->_carlaMap->GenerateWaypoints(0.1);
        auto ss_ = std::stringstream();
        ss_ << "\n====GenerateWaypoints(0.1) 根据 0.1 生成路点，长度 " << res_.size() << "\n";
        // for (size_t i_ = 0; i_ < res_.size(); i_++) {
        //     ss_ << "GenerateWaypoints(1)[" << i_ << "]." << "GetAllLandmarksInDistance(10, flase) 长度 " << res_[i_]->GetAllLandmarksInDistance(1, false).size() << "\n";
        //     ss_ << "GenerateWaypoints(1)[" << i_ << "]." << "GetDistance() = " << res_[i_]->GetDistance();
        //     ss_ << "GenerateWaypoints(1)[" << i_ << "]." << "GetId() = " << res_[i_]->GetId();
        //     ss_ << "GenerateWaypoints(1)[" << i_ << "]." << "GetJunction().GetBoundingBox().location.x = " << res_[i_]->GetJunction()->GetBoundingBox().location.x << "\n";
        //     ss_ << "GenerateWaypoints(1)[" << i_ << "]." << "GetJunction().GetId() = " << res_[i_]->GetJunction()->GetId() << "\n";
        // }
        LOG(INFO) << ss_.str();
    } else if (double_clicked_item_text_ == "GenerateOpenDriveWorld") {
        this->_GenerateOpenDriveWorld();
    } else if (double_clicked_item_text_ == "GetAvailableMaps") {
        auto res_ = this->_carlaClient->GetAvailableMaps();
        auto ss_ = std::stringstream();
        ss_ << "\n====GetAvailableMaps() 获取可用的地图，数量 " << res_.size() << "\n";
        for (auto i_: res_) {
            ss_ << i_ << "\n";
        }
        LOG(INFO) << ss_.str();
    } else if (double_clicked_item_text_ == "glogTest") {
        LOG(INFO) << "INFO Logqqqq";
        LOG(WARNING) << "WARNING Logdfdfdsaa";
        LOG(ERROR) << "ERROR Logdfdf";
    } else if (double_clicked_item_text_ == "GetAndShowRecommendedSpawnPoints") {
        this->_GetAndShowRecommendedSpawnPoints();
    } else if (double_clicked_item_text_ == "SpawnACarInSelectedPoint") {
        this->_SpawnACarInSelectedPoint();
    } else if (double_clicked_item_text_ == "DestoryAllCar") {
        this->_DestoryAllCar();
    } else if (double_clicked_item_text_ == "SpawnACarInTargetTransform") {
        this->_SpawnACarInTargetTransform();
    } else if (double_clicked_item_text_ == "GetAllActorsInfo") {
        this->_GetAllActorsInfo();
    } else if (double_clicked_item_text_ == "设置当前要控制的主车") {
        this->SetCurrentEgoCar();
    } else if (double_clicked_item_text_ == "获取当前主车周围的环境信息") {
        this->GetCurrentEgoCarEnv();
    }
}

void MainWindow::_SlotOnListWidgetItemPressed(QListWidgetItem* item)
{
    std::cout << "_SlotOnListWidgetItemPressed" << item->text().toStdString() << std::endl;
}

void MainWindow::_WriteToFile(const std::string& file_name, const std::string& file_text)
{
    try
    {
        /* code */
        std::fstream f_;
        f_.open(file_name, std::ios::out);
        f_ << file_text;
        f_.close();
    }
    catch(const std::exception& e)
    {
        std::cerr << "MainWindow::_WriteToFile " << e.what() << '\n';
    }
}

uint64_t MainWindow::_GetNowTimeCount()
{
    return std::chrono::system_clock::now().time_since_epoch().count();
}

void MainWindow::_EchoAndSave(const std::stringstream& s_stream, const std::string& file_name)
{
    // std::cout << s_stream.str() << std::endl;
    // LOG(INFO) << s_stream.str() << std::endl;
    // this->_WriteToFile(file_name, s_stream.str());
}

int MainWindow::_ReadFile(const std::string& file_path, std::stringstream& file_context)
{
    try
    {
        auto f_ = std::fstream(file_path, std::ios::in);
        f_.seekg(0, std::ios_base::end);
        uint64_t len_ = f_.tellg();
        f_.seekg(std::ios::beg);
        char* buffer_ = new char[len_ + 1]();
        f_.read(buffer_, len_ + 1);
        file_context << buffer_;
        return 0;
    }
    catch(const std::exception& e)
    {
        std::cerr << "MainWindow::_ReadFile" << file_path << " Error: " << e.what() << '\n';
        return 1;
    }
}

void MainWindow::_GenerateOpenDriveWorld()
{
    auto xodr_file_path_ = QFileDialog::getOpenFileName(this, "请选择打开的 xodr 文件", "", "*.xodr");
    if (xodr_file_path_.length() == 0) {
        return;
    }
    auto xodr_context_s_ = std::stringstream();
    if (this->_ReadFile(xodr_file_path_.toStdString(), xodr_context_s_) != 0) {
        return;
    }
    this->_carlaWorld = std::make_shared<carla::client::World>(this->_carlaClient->GenerateOpenDriveWorld(
        xodr_context_s_.str(), 
        carla::rpc::OpendriveGenerationParameters()));
    this->_carlaMap = this->_carlaWorld->GetMap();
    return;
}

void MainWindow::_GetAndShowRecommendedSpawnPoints()
{
    this->_recommandSpawnPoints = std::make_shared<std::vector<carla::geom::Transform>>(
        this->_carlaMap->GetRecommendedSpawnPoints()
    );
    auto ss_ = std::stringstream();
    ss_ << "\n====GetAndShowRecommendedSpawnPoints 获取当前地图建议的出生点， 长度 " 
        << this->_recommandSpawnPoints->size() << "\n";
    for (size_t i_ = 0; i_ < this->_recommandSpawnPoints->size(); i_++) {
        ss_ << "[" << i_ << "]" 
            << "\n"
            << "location.xyz = "
            << this->_recommandSpawnPoints->at(i_).location.x
            << this->_recommandSpawnPoints->at(i_).location.y
            << this->_recommandSpawnPoints->at(i_).location.z
            << " "
            << "rotation.ryp = "
            << this->_recommandSpawnPoints->at(i_).rotation.roll
            << this->_recommandSpawnPoints->at(i_).rotation.yaw
            << this->_recommandSpawnPoints->at(i_).rotation.pitch
            << "\n";
    }
    LOG(INFO) << ss_.str();
}

void MainWindow::_SpawnACarInSelectedPoint()
{
    auto current_spawn_point_count_ = this->_recommandSpawnPoints->size();
    auto ss_ = std::stringstream();
    ss_ << "从 " << 0 << " 到 " << current_spawn_point_count_;
    auto is_get_ = false;
    auto spawn_point_index_ = QInputDialog::getInt(
        this,
        "请输入要出生的点",
        QString::fromStdString(ss_.str()),
        0,
        0,
        current_spawn_point_count_,
        1,
        &is_get_
    );
    if (! is_get_) {
        return;
    }
    auto spawn_point_ = this->_recommandSpawnPoints->at(spawn_point_index_);
    this->_CreateACar(spawn_point_);
}

void MainWindow::_DestoryAllCar()
{
    auto current_acts_ = this->_carlaWorld->GetActors();
    auto current_vehicle_ = current_acts_->Filter("vehicle.*");
    // for ()
    LOG(INFO) << "找到当前的 Vehicle 数量为 " << current_acts_->size();
    for (size_t i_ = 0; i_ < current_vehicle_->size(); i_++) {
        auto act_ = current_acts_->at(i_);
        // act_->GetAttributes().at(0).
        auto act_vehicle = boost::static_pointer_cast<carla::client::Vehicle>(act_);
        LOG(INFO) << "_DestoryAllCar 查找要 Destory 的车辆成功" << std::endl;
        LOG(INFO) 
            << "loaction xyz = " 
            << act_vehicle->GetLocation().x 
            << act_vehicle->GetLocation().y 
            << act_vehicle->GetLocation().z 
            << std::endl;
        act_vehicle->Destroy();
    }
}

boost::shared_ptr<carla::client::Vehicle> 
    MainWindow::_CreateACar(const carla::geom::Transform& spawn_point)
{
    auto bpl_ = this->_carlaWorld->GetBlueprintLibrary();
    auto bp_model3_ = bpl_->Find("vehicle.tesla.model3");
    assert (bp_model3_), "找不到蓝图 vehicle.tesla.model3";
    auto act_ = this->_carlaWorld->SpawnActor(*bp_model3_, spawn_point);
    return boost::static_pointer_cast<carla::client::Vehicle>(act_);
}

void MainWindow::_SpawnACarInTargetTransform()
{
    auto is_valid_ = false;
    auto s_transform_ = QInputDialog::getText(
        this,
        "请输入要创建车辆的位置",
        "x,y,z,pitch,yaw,roll",
        QLineEdit::Normal,
        "",
        &is_valid_
    );
    if (! is_valid_) {
        LOG(INFO) << "无效的出生点";
        return;
    }
    auto s_transoform_split_ =  s_transform_.split(',');
    if (s_transoform_split_.length() != 6) {
        LOG(ERROR) << "车辆位置信息不够，应该输入 6 个";
        return;
    }
    auto location_ix = s_transoform_split_.at(0).toFloat();
    auto location_iy = s_transoform_split_.at(1).toFloat();
    auto location_iz = s_transoform_split_.at(2).toFloat();
    auto rotation_pitch_ = s_transoform_split_.at(3).toFloat();
    auto rotation_yaw_ = s_transoform_split_.at(4).toFloat();
    auto rotation_roll_ = s_transoform_split_.at(5).toFloat();
    this->_CreateACar(carla::geom::Transform(
        carla::geom::Location(location_ix, location_iy, location_iz),
        carla::geom::Rotation(rotation_pitch_, rotation_yaw_, rotation_roll_)
    ));
}

void MainWindow::_GetAllActorsInfo()
{
    auto current_acts_ = this->_carlaWorld->GetActors();
    std::stringstream ss_;
    ss_ << "GetAllActorsInfo======\n";
    for (size_t i_ = 0; i_ < current_acts_->size(); i_++) {
        auto act_ = current_acts_->at(i_);
        ss_ << "GetAllActorsInfo[" << i_ << "]" << "\n";
        ss_ << "Actor.GetId() = " << act_->GetId() << "\n";
        ss_ << "Actor.GetTypeId() = " << act_->GetTypeId() << "\n";
        ss_ << "Actor.GetActorState() = " << (uint8_t)act_->GetActorState() << "\n";
        auto attrs_ = act_->GetAttributes();
        for (auto attr_: attrs_) {
            ss_ << attr_.GetId() << ": " << attr_.GetValue() << "\n";       
        }
    }
    LOG(INFO) << ss_.str();
}

